// /*
//  * Copyright (c) 2006-2024, RT-Thread Development Team
//  *
//  * SPDX-License-Identifier: Apache-2.0
//  *
//  * Change Logs:
//  * Date           Author       Notes
//  * 2024-07-22     RT-Thread    first version
//  */

// #include <rtthread.h>
// #include "board.h"
// #define DBG_TAG "main"
// #define DBG_LVL DBG_LOG
// #include <rtdbg.h>

// #include "mpu6xxx.h"
// #include "mpu6050.h"
// #include "uart.h"

// #include "at.h"

// #define serialName ("uart2")
// #define otaBuffLen (2048)
// #define atBufSize (otaBuffLen + 100)

// at_client_t at_client;
// at_response_t at_response;
// uint8_t startFlag = 1;

// /* Default configuration, please change according to the actual situation, support i2c and spi device name */
// #define MPU6XXX_DEVICE_NAME "i2c2"

// #define start_word "+++"
// #define end_word "a"

// static void start_callback(void)
// {
//     startFlag = 0;
//     rt_kprintf("start_callback\n");
//     while (at_obj_exec_cmd(at_client, at_response, "AT+WMODE=STA"))
//     {
//     }
//     while (at_obj_exec_cmd(at_client, at_response, "AT+WSSSID=kfyjs-5G"))
//     {
//     }
//     while (at_obj_exec_cmd(at_client, at_response, "AT+WSKEY=WPA2PSK,AES,13af8888"))
//     {
//     }
//     while (at_obj_exec_cmd(at_client, at_response, "AT+NETP=TCP,Client,8899,172.16.10.23"))
//     {
//     }
// }

// /**
//  * @brief       传送数据给 ANO_TC匿名科创地面站v4.exe
//  * @param       fun  : 功能字. 0XA0~0XAF
//  * @param       data : 数据缓存区,最多28字节!!
//  * @param       len  : data区有效数据个数
//  * @retval      无
//  */
// void usart1_niming_report(uint8_t fun, uint8_t *data, uint8_t len)
// {
//     uint8_t send_buf[32];
//     uint8_t i;

//     if (len > 28)
//     {
//         return; /* 最多28字节数据 */
//     }

//     send_buf[len + 4] = 0; /* 校验数置零 */
//     send_buf[0] = 0XAA;    /* 帧头 */
//     send_buf[1] = 0XAA;    /* 帧头 */
//     send_buf[2] = fun;     /* 功能字 */
//     send_buf[3] = len;     /* 数据长度 */

//     for (i = 0; i < len; i++)
//     {
//         send_buf[4 + i] = data[i]; /* 复制数据 */
//     }

//     for (i = 0; i < len + 4; i++)
//     {
//         send_buf[len + 4] += send_buf[i]; /* 计算校验和 */
//     }

//     uart_send(send_buf, (len + 5));
// }

// /**
//  * @brief       通过串口1上报结算后的姿态数据给电脑
//  * @param       roll     : 横滚角.单位0.1度。 -9000 -> 9000 对应 -90.00  ->  90.00度
//  * @param       pitch    : 俯仰角.单位 0.1度。-18000 -> 18000 对应 -180.00 -> 180.00 度
//  * @param       yaw      : 航向角.单位为0.1度 -18000 -> 18000  对应 -180.00 -> 180.00 度
//  * @param       prs      : 气压计高度,单位:cm
//  * @param       fly_mode : 飞行模式
//  * @param       armed    : 锁定状态
//  * @retval      无
//  */
// void usart1_report_imu(short roll, short pitch, short yaw, int prs, uint8_t fly_mode, uint8_t armed)
// {
//     uint8_t tbuf[12];

//     tbuf[0] = (roll >> 8) & 0XFF;
//     tbuf[1] = roll & 0XFF;
//     tbuf[2] = (pitch >> 8) & 0XFF;
//     tbuf[3] = pitch & 0XFF;
//     tbuf[4] = (yaw >> 8) & 0XFF;
//     tbuf[5] = yaw & 0XFF;
//     tbuf[6] = (prs >> 24) & 0XFF;
//     tbuf[7] = (prs >> 16) & 0XFF;
//     tbuf[8] = (prs >> 8) & 0XFF;
//     tbuf[9] = prs & 0XFF;
//     tbuf[10] = fly_mode;
//     tbuf[11] = armed;
//     usart1_niming_report(0X01, tbuf, 12); /* 状态帧,0X01 */
// }
// /**
//  * @brief       发送加速度传感器数据和陀螺仪数据
//  * @param       aacx,aacy,aacz    : x,y,z三个方向上面的加速度值
//  * @param       gyrox,gyroy,gyroz : x,y,z三个方向上面的陀螺仪值
//  * @retval      无
//  */
// void sh3001_send_data(short aacx, short aacy, short aacz, short gyrox, short gyroy, short gyroz)
// {
//     uint8_t tbuf[18];
//     tbuf[0] = (aacx >> 8) & 0XFF;
//     tbuf[1] = aacx & 0XFF;
//     tbuf[2] = (aacy >> 8) & 0XFF;
//     tbuf[3] = aacy & 0XFF;
//     tbuf[4] = (aacz >> 8) & 0XFF;
//     tbuf[5] = aacz & 0XFF;
//     tbuf[6] = (gyrox >> 8) & 0XFF;
//     tbuf[7] = gyrox & 0XFF;
//     tbuf[8] = (gyroy >> 8) & 0XFF;
//     tbuf[9] = gyroy & 0XFF;
//     tbuf[10] = (gyroz >> 8) & 0XFF;
//     tbuf[11] = gyroz & 0XFF;
//     tbuf[12] = 0; /* 因为开启MPL后,无法直接读取磁力计数据,所以这里直接屏蔽掉.用0替代. */
//     tbuf[13] = 0;
//     tbuf[14] = 0;
//     tbuf[15] = 0;
//     tbuf[16] = 0;
//     tbuf[17] = 0;
//     usart1_niming_report(0X02, tbuf, 18); /* 自定义帧,0X02 */
// }

// static struct at_urc mqttUrcTable[] =
//     {
//         {"a+ok", "\r\n", start_callback},
// };

// short g_acc_avg[3];  /* 加速度平均值 */
// short g_gyro_avg[3]; /* 陀螺仪平均值 */

// /**
//  * @brief       数据校准
//  * @note        对数据减去初值, 加速度减去重力加速度影响
//  * @param       gx, gy, gz  : 3轴陀螺仪数据指针
//  * @param       ax, ay, az  : 3轴加速度数据指针
//  * @retval      无
//  */
// void imu_data_calibration(short *gx, short *gy, short *gz, short *ax, short *ay, short *az)
// {
//     /* 减去初值(去掉零飘) */
//     *gx -= g_gyro_avg[0];
//     *gy -= g_gyro_avg[1];
//     *gz -= g_gyro_avg[2];
//     *ax -= g_acc_avg[0];
//     *ay -= g_acc_avg[1];
//     *az -= (g_acc_avg[2] - 2048); /* 2048是重力加速度理论值 */
// }

// #include <math.h>

// #define M_PI 3.1425f
// // 假设这是你的数据结构
// typedef struct
// {
//     float ax, ay, az;       // 加速度计数据
//     float gx, gy, gz;       // 陀螺仪数据 (度/秒)
//     float roll, pitch, yaw; // 欧拉角 (度)
// } SensorData;

// // 这个函数用来计算去除重力加速度影响的线性加速度
// void removeGravity(SensorData *data)
// {
//     // 将欧拉角从度转换为弧度
//     float roll_rad = data->roll * M_PI / 180.0;
//     float pitch_rad = data->pitch * M_PI / 180.0;

//     // 重力加速度分量在三个轴上的投影
//     float gravity_x = sin(roll_rad) * sin(pitch_rad);
//     float gravity_y = cos(roll_rad) * sin(pitch_rad);
//     float gravity_z = cos(pitch_rad);

//     // 重力加速度大小（通常为9.8 m/s^2）
//     const float gravity = 9.8f;

//     // 计算去除重力影响后的加速度
//     data->ax -= gravity * gravity_x;
//     data->ay -= gravity * gravity_y;
//     data->az -= gravity * gravity_z;
// }

// // 从MPU6050读取加速度计和陀螺仪数据
// void read_mpu6050(float *ax, float *ay, float *az, float *gx, float *gy, float *gz)
// {

//     *ax = *ax / 16384.0f; // 转换为g
//     *ay = *ay / 16384.0f;
//     *az = *az / 16384.0f;
//     *gx = *gx / 131.0f; // 转换为度/秒
//     *gy = *gy / 131.0f;
//     *gz = *gz / 131.0f;
// }

// // 计算欧拉角
// void calculate_euler_angles(float ax, float ay, float az, float *pitch, float *roll)
// {
//     *pitch = atan2(-ay, sqrt(ax * ax + az * az)) * 180.0f / M_PI;
//     *roll = atan2(az, sqrt(ay * ay + ax * ax)) * 180.0f / M_PI;
// }

// // 构建旋转矩阵
// void build_rotation_matrix(float pitch, float roll, float R[3][3])
// {
//     float sp = sin(pitch * M_PI / 180.0f);
//     float cp = cos(pitch * M_PI / 180.0f);
//     float sr = sin(roll * M_PI / 180.0f);
//     float cr = cos(roll * M_PI / 180.0f);

//     R[0][0] = cp * cr;
//     R[0][1] = cp * sr;
//     R[0][2] = -sp;
//     R[1][0] = -sp * sr;
//     R[1][1] = sp * cr;
//     R[1][2] = cp;
//     R[2][0] = sr;
//     R[2][1] = -cr;
//     R[2][2] = 0;
// }

// // 应用旋转矩阵
// void apply_rotation_matrix(float v[3], float R[3][3], float result[3])
// {
//     result[0] = R[0][0] * v[0] + R[0][1] * v[1] + R[0][2] * v[2];
//     result[1] = R[1][0] * v[0] + R[1][1] * v[1] + R[1][2] * v[2];
//     result[2] = R[2][0] * v[0] + R[2][1] * v[1] + R[2][2] * v[2];
// }

// // 互补滤波器姿态估计
// void complementary_filter(float ax, float ay, float az, float gx, float gy, float gz,
//                           float *pitch, float *roll, float *yaw, float dt)
// {
//     static float last_pitch = 0;
//     static float last_roll = 0;
//     static float last_yaw = 0;

//     // 更新欧拉角
//     float new_pitch, new_roll;
//     calculate_euler_angles(ax, ay, az, &new_pitch, &new_roll);

//     // 陀螺仪角度更新
//     *pitch += (gx * dt + 0.005f * (new_pitch - last_pitch));
//     *roll += (gy * dt + 0.005f * (new_roll - last_roll));
//     *yaw += gz * dt;

//     // 更新上一次的角度
//     last_pitch = *pitch;
//     last_roll = *roll;
//     last_yaw = *yaw;
// }

// int main(void)
// {

//     struct mpu6xxx_device *dev;
//     struct mpu6xxx_3axes accel, gyro;
//     eulerian_angles_t e_angles;
//     short acc_data[3];
//     short gyro_data[3];

//     float ax, ay, az, gx, gy, gz;
//     float pitch, roll, yaw;
//     float R[3][3];
//     float gravity[3] = {0, 0, 9.81};
//     float gravity_device[3];
//     float linear_acceleration[3];
//     float dt = 0.005f; // 假设采样时间为5ms

//     int acc_sum[3] = {0}, gyro_sum[3] = {0};

//     dev = mpu6xxx_init(MPU6XXX_DEVICE_NAME, RT_NULL);

//     mpu6xxx_set_param(dev, MPU6XXX_ACCEL_RANGE, MPU6XXX_ACCEL_RANGE_2G);
//     mpu6xxx_set_param(dev, MPU6XXX_GYRO_RANGE, MPU6XXX_GYRO_RANGE_250DPS);

//     mpu6xxx_set_param(dev, MPU6XXX_DLPF_CONFIG, MPU6XXX_DLPF_188HZ);
//     mpu6xxx_set_param(dev, MPU6XXX_SAMPLE_RATE, 100);

//     for (int i = 0; i < 250; i++) /* 循环读取250次 取平均 */
//     {
//         mpu6xxx_get_accel(dev, &accel);

//         mpu6xxx_get_gyro(dev, &gyro);

//         acc_sum[0] += accel.x;
//         acc_sum[1] += accel.y;
//         acc_sum[2] += accel.z;
//         gyro_sum[0] += gyro.x;
//         gyro_sum[1] += gyro.y;
//         gyro_sum[2] += gyro.z;

//         rt_thread_mdelay(5);
//     }

//     g_acc_avg[0] = acc_sum[0] / 250;
//     g_acc_avg[1] = acc_sum[1] / 250;
//     g_acc_avg[2] = acc_sum[2] / 250;
//     g_gyro_avg[0] = gyro_sum[0] / 250;
//     g_gyro_avg[1] = gyro_sum[1] / 250;
//     g_gyro_avg[2] = gyro_sum[2] / 250;

//     while (1)
//     {

//         mpu6xxx_get_accel(dev, &accel);
//         mpu6xxx_get_gyro(dev, &gyro);

//         imu_data_calibration(&gyro.x,
//                              &gyro.y,
//                              &gyro.z,
//                              &accel.x,
//                              &accel.y,
//                              &accel.z);

//         // rt_kprintf("Accel: %3d %3d %3d\n", accel.x, accel.y, accel.z);

//         ax = accel.x * 1.0f;
//         ay = accel.y * 1.0f;
//         az = accel.z * 1.0f;
//         gx = gyro.x * 1.0f;
//         gy = gyro.y * 1.0f;
//         gz = gyro.z * 1.0f;

//         // 读取加速度计和陀螺仪数据
//         read_mpu6050(&ax, &ay, &az, &gx, &gy, &gz);

//         // 使用互补滤波器进行姿态估计
//         complementary_filter(ax, ay, az, gx, gy, gz, &pitch, &roll, &yaw, dt);

//         // 构建旋转矩阵
//         build_rotation_matrix(pitch, roll, R);

//         // 应用旋转矩阵以得到设备坐标系下的重力加速度
//         apply_rotation_matrix(gravity, R, gravity_device);

//         // 从加速度计读数中去除重力加速度
//         linear_acceleration[0] = ax * 9.81 - gravity_device[0];
//         linear_acceleration[1] = ay * 9.81 - gravity_device[1];
//         linear_acceleration[2] = az * 9.81 - gravity_device[2];

//         // 输出去除重力加速度后的加速度
//         printf("Linear Acceleration: %f, %f, %f\n", linear_acceleration[0], (linear_acceleration[1] + 9.8f), linear_acceleration[2]);

//         rt_thread_mdelay(5);
//     }

//     return RT_EOK;
// }

// // #include <stdio.h>
// // #include <stdlib.h>
// // #include <math.h>

// // // 假设这是MPU6050的库文件
// // #include "mpu6050.h"

// // // 定义MPU6050的地址
// // #define MPU6050_ADDR 0x68

// // // 从MPU6050读取加速度计和陀螺仪数据
// // void read_mpu6050(float *ax, float *ay, float *az, float *gx, float *gy, float *gz)
// // {
// //     mpu6050_data_t data;
// //     mpu6050_get_data(MPU6050_ADDR, &data);
// //     *ax = data.ax / 16384.0f; // 转换为g
// //     *ay = data.ay / 16384.0f;
// //     *az = data.az / 16384.0f;
// //     *gx = data.gx / 131.0f; // 转换为度/秒
// //     *gy = data.gy / 131.0f;
// //     *gz = data.gz / 131.0f;
// // }

// // // 计算欧拉角
// // void calculate_euler_angles(float ax, float ay, float az, float *pitch, float *roll)
// // {
// //     *pitch = atan2(-ay, sqrt(ax * ax + az * az)) * 180.0f / M_PI;
// //     *roll = atan2(az, sqrt(ay * ay + ax * ax)) * 180.0f / M_PI;
// // }

// // // 构建旋转矩阵
// // void build_rotation_matrix(float pitch, float roll, float R[3][3])
// // {
// //     float sp = sin(pitch * M_PI / 180.0f);
// //     float cp = cos(pitch * M_PI / 180.0f);
// //     float sr = sin(roll * M_PI / 180.0f);
// //     float cr = cos(roll * M_PI / 180.0f);

// //     R[0][0] = cp * cr;
// //     R[0][1] = cp * sr;
// //     R[0][2] = -sp;
// //     R[1][0] = -sp * sr;
// //     R[1][1] = sp * cr;
// //     R[1][2] = cp;
// //     R[2][0] = sr;
// //     R[2][1] = -cr;
// //     R[2][2] = 0;
// // }

// // // 应用旋转矩阵
// // void apply_rotation_matrix(float v[3], float R[3][3], float result[3])
// // {
// //     result[0] = R[0][0] * v[0] + R[0][1] * v[1] + R[0][2] * v[2];
// //     result[1] = R[1][0] * v[0] + R[1][1] * v[1] + R[1][2] * v[2];
// //     result[2] = R[2][0] * v[0] + R[2][1] * v[1] + R[2][2] * v[2];
// // }

// // int main()
// // {

// //     // 初始化MPU6050
// //     init_mpu6050();

// //     while (1)
// //     {

// //         // 适当延时
// //         usleep(100000); // 100ms
// //     }

// //     return 0;
// // }

// #include <stdio.h>
// #include <stdlib.h>
// #include <math.h>

// // 假设这是MPU6050的库文件
// #include "mpu6050.h"

// // 定义MPU6050的地址
// #define MPU6050_ADDR 0x68

// // 从MPU6050读取加速度计和陀螺仪数据
// void read_mpu6050(float *ax, float *ay, float *az, float *gx, float *gy, float *gz)
// {
//     mpu6050_data_t data;
//     mpu6050_get_data(MPU6050_ADDR, &data);
//     *ax = data.ax / 16384.0f; // 转换为g
//     *ay = data.ay / 16384.0f;
//     *az = data.az / 16384.0f;
//     *gx = data.gx / 131.0f; // 转换为度/秒
//     *gy = data.gy / 131.0f;
//     *gz = data.gz / 131.0f;
// }

// // 计算欧拉角
// void calculate_euler_angles(float ax, float ay, float az, float *pitch, float *roll)
// {
//     *pitch = atan2(-ay, sqrt(ax * ax + az * az)) * 180.0f / M_PI;
//     *roll = atan2(az, sqrt(ay * ay + ax * ax)) * 180.0f / M_PI;
// }

// // 构建旋转矩阵
// void build_rotation_matrix(float pitch, float roll, float R[3][3])
// {
//     float sp = sin(pitch * M_PI / 180.0f);
//     float cp = cos(pitch * M_PI / 180.0f);
//     float sr = sin(roll * M_PI / 180.0f);
//     float cr = cos(roll * M_PI / 180.0f);

//     R[0][0] = cp * cr;
//     R[0][1] = cp * sr;
//     R[0][2] = -sp;
//     R[1][0] = -sp * sr;
//     R[1][1] = sp * cr;
//     R[1][2] = cp;
//     R[2][0] = sr;
//     R[2][1] = -cr;
//     R[2][2] = 0;
// }

// // 应用旋转矩阵
// void apply_rotation_matrix(float v[3], float R[3][3], float result[3])
// {
//     result[0] = R[0][0] * v[0] + R[0][1] * v[1] + R[0][2] * v[2];
//     result[1] = R[1][0] * v[0] + R[1][1] * v[1] + R[1][2] * v[2];
//     result[2] = R[2][0] * v[0] + R[2][1] * v[1] + R[2][2] * v[2];
// }

// int main()
// {

//     // 初始化MPU6050
//     init_mpu6050();

//     while (1)
//     {

//         // 适当延时
//         usleep(100000); // 100ms
//     }

//     return 0;
// }









//2024/8/13   
/*
 * Copyright (c) 2006-2024, RT-Thread Development Team
 *
 * SPDX-License-Identifier: Apache-2.0
 *
 * Change Logs:
 * Date           Author       Notes
 * 2024-07-22     RT-Thread    first version
 */

// #include <rtthread.h>
// #include "board.h"
// #define DBG_TAG "main"
// #define DBG_LVL DBG_LOG
// #include <rtdbg.h>

// #include "mpu6xxx.h"
// #include "mpu6050.h"
// #include "uart.h"

// #include "at.h"
// #include <math.h>

// #define MPU6XXX_DEVICE_NAME "i2c2"
// #define M_PI (3.14159265358979323846)

// // 量程设置为±2g
// #define G_SCALE (2.0f)                   // g量程
// #define SENSITIVITY (16384.0f / G_SCALE) // LSB/g

// #define gravity 9.8f // 重力加速度大小（通常为9.8 m/s^2）

// short g_acc_avg[3];  /* 加速度平均值 */
// short g_gyro_avg[3]; /* 陀螺仪平均值 */

// /**
//  * @brief       数据校准
//  * @note        对数据减去初值, 加速度减去重力加速度影响
//  * @param       gx, gy, gz  : 3轴陀螺仪数据指针
//  * @param       ax, ay, az  : 3轴加速度数据指针
//  * @retval      无
//  */
// void imu_data_calibration(short *gx, short *gy, short *gz, short *ax, short *ay, short *az)
// {
//     /* 减去初值(去掉零飘) */
//     *gx -= g_gyro_avg[0];
//     *gy -= g_gyro_avg[1];
//     *gz -= g_gyro_avg[2];
//     *ax -= g_acc_avg[0];
//     *ay -= g_acc_avg[1];
//     *az -= (g_acc_avg[2] - 2048); /* 2048是重力加速度理论值 */
//     // *az -= g_acc_avg[2];
// }

// // 应用旋转矩阵
// void apply_rotation_matrix(float v[3], float R[3][3], float result[3])
// {
//     result[0] = R[0][0] * v[0] + R[0][1] * v[1] + R[0][2] * v[2];
//     result[1] = R[1][0] * v[0] + R[1][1] * v[1] + R[1][2] * v[2];
//     result[2] = R[2][0] * v[0] + R[2][1] * v[1] + R[2][2] * v[2];
// }

// // 构建旋转矩阵
// void build_rotation_matrix(float pitch, float roll, float yaw, float R[3][3])
// {
//     float sp = sin(pitch * M_PI / 180.0f);
//     float cp = cos(pitch * M_PI / 180.0f);
//     float sr = sin(roll * M_PI / 180.0f);
//     float cr = cos(roll * M_PI / 180.0f);
//     float sy = sin(yaw * M_PI / 180.0f);
//     float cy = cos(yaw * M_PI / 180.0f);

//     R[0][0] = cp * cy;
//     R[0][1] = (sr * sp * cy) - (cr * sy);
//     R[0][2] = (cr * sp * cy) + (sr * sy);
//     R[1][0] = cp * sy;
//     R[1][1] = (sr * sp * sy) + (cr * cy);
//     R[1][2] = (cr * sp * sy) - (sr * cy);
//     R[2][0] = -sp;
//     R[2][1] = sr * cp;
//     R[2][2] = cr * cp;
// }

// // 修改后的calculate_linear_acceleration函数
// void calculate_linear_acceleration(float ax, float ay, float az, float g_device[3], float *linear_acceleration)
// {
//     // 由于加速度计读数包含重力分量，我们需要从中减去重力的影响，
//     // 以得到线性加速度，即只反映物体运动的加速度。

//     // 由于加速度计读数是以g为单位，因此需要将g_device也转换为相同的单位
//     float g_device_in_g[3];
//     g_device_in_g[0] = g_device[0] / gravity;
//     g_device_in_g[1] = g_device[1] / gravity;
//     g_device_in_g[2] = g_device[2] / gravity;

//     // 线性加速度 = 加速度计读数 - 重力向量（在传感器坐标系下）
//     linear_acceleration[0] = ax - g_device_in_g[0];
//     linear_acceleration[1] = ay - g_device_in_g[1];
//     linear_acceleration[2] = az - g_device_in_g[2];
// }

// #include <math.h>

// typedef struct
// {
//     float ax; // X轴加速度
//     float ay; // Y轴加速度
//     float az; // Z轴加速度
// } accelerometer_data_t;

// // 计算瞬时加速度的函数
// void calculateInstantaneousAcceleration(eulerian_angles_t euler_angles, accelerometer_data_t accel_data, accelerometer_data_t *instantaneous_acceleration)
// {
//     // 将欧拉角从度转换为弧度
//     float pitch_rad = euler_angles.pitch * M_PI / 180.0;
//     float roll_rad = euler_angles.roll * M_PI / 180.0;
//     float yaw_rad = euler_angles.yaw * M_PI / 180.0;

//     // 重力加速度
//     const float g = 9.80665f; // m/s^2

//     // 计算重力向量在设备坐标系中的分量
//     float g_x = g * sin(pitch_rad) * cos(yaw_rad) + g * sin(roll_rad) * sin(yaw_rad);
//     float g_y = g * sin(pitch_rad) * sin(yaw_rad) - g * sin(roll_rad) * cos(yaw_rad);
//     float g_z = g * cos(pitch_rad);

//     printf("g_x = %f, g_y = %f, g_z = %f\n", g_x, g_y, g_z);

//     // 从加速度计读数中分离出重力加速度

//     instantaneous_acceleration->ax = accel_data.ax - g_x;
//     instantaneous_acceleration->ay = accel_data.ay - g_y;
//     instantaneous_acceleration->az = accel_data.az - g_z;
// }

// /**
//  * @brief       从加速度计读数中分离重力分量
//  * @param       ax, ay, az : 加速度计读数
//  * @param       out_ax, out_ay, out_az : 动态加速度输出
//  * @retval      无
//  */

// int main(void)
// {

//     struct mpu6xxx_device *dev;
//     struct mpu6xxx_3axes accel, gyro, temp;
//     eulerian_angles_t e_angles;
//     int acc_sum[3] = {0}, gyro_sum[3] = {0};

//     // 旋转矩阵和结果数组
//     float R[3][3];
//     float g_device[3]; // 设备坐标系中的重力向量
//     float linear_acceleration[3];
//     short acc_data[3];
//     short gyro_data[3];
//     float gravity_world[3] = {0.0f, 0.0f, 1.0f}; // 世界坐标系中的重力向量
//     accelerometer_data_t instantaneous_acceleration;

//     dev = mpu6xxx_init(MPU6XXX_DEVICE_NAME, RT_NULL);

//     // mpu6xxx_set_param(dev, MPU6XXX_ACCEL_RANGE, MPU6XXX_ACCEL_RANGE_2G);
//     // mpu6xxx_set_param(dev, MPU6XXX_GYRO_RANGE, MPU6XXX_GYRO_RANGE_250DPS);

//     // mpu6xxx_set_param(dev, MPU6XXX_DLPF_CONFIG, MPU6XXX_DLPF_188HZ);
//     // mpu6xxx_set_param(dev, MPU6XXX_SAMPLE_RATE, 1000);

//     for (int i = 0; i < 250; i++)
//     {
//         mpu6xxx_get_accel(dev, &accel);
//         mpu6xxx_get_gyro(dev, &gyro);

//         // rt_kprintf("accel.x = %3d, accel.y = %3d, accel.z = %3d ", accel.x, accel.y, accel.z);
//         // rt_kprintf("gyro.x = %3d gyro.y = %3d, gyro.z = %3d\n", gyro.x, gyro.y, gyro.z);

//         acc_sum[0] += accel.x;
//         acc_sum[1] += accel.y;
//         acc_sum[2] += accel.z;
//         gyro_sum[0] += gyro.x;
//         gyro_sum[1] += gyro.y;
//         gyro_sum[2] += gyro.z;

//         rt_thread_mdelay(5);
//     }

//     g_acc_avg[0] = acc_sum[0] / 250;
//     g_acc_avg[1] = acc_sum[1] / 250;
//     g_acc_avg[2] = acc_sum[2] / 250;
//     g_gyro_avg[0] = gyro_sum[0] / 250;
//     g_gyro_avg[1] = gyro_sum[1] / 250;
//     g_gyro_avg[2] = gyro_sum[2] / 250;

//     while (1)
//     {

//         mpu6xxx_get_accel(dev, &accel);
//         mpu6xxx_get_gyro(dev, &gyro);

//         // rt_kprintf("accel.x = %3d, accel.y = %3d, accel.z = %3d ", accel.x, accel.y, accel.z);
//         // rt_kprintf("gyro.x = %3d gyro.y = %3d, gyro.z = %3d\n", gyro.x, gyro.y, gyro.z);

//         acc_data[0] = accel.x;
//         acc_data[1] = accel.y;
//         acc_data[2] = accel.z;
//         gyro_data[0] = gyro.x;
//         gyro_data[1] = gyro.y;
//         gyro_data[2] = gyro.z;

//         /* 数据校准 */
//         imu_data_calibration(&gyro_data[0], &gyro_data[1], &gyro_data[2],
//                              &acc_data[0], &acc_data[1], &acc_data[2]);

//         mpu_get_gravity(gyro_data[0], gyro_data[1], gyro_data[2], acc_data[0], acc_data[1], acc_data[2],
//                         accel.x / 100.f, accel.y / 100.f, accel.z / 100.f);

//         // temp = accel;

//         // // // 将加速度计读数转换为g单位
//         // float ax = accel.x / 100.0f;
//         // float ay = accel.y / 100.0f;
//         // float az = accel.z / 100.0f;

//         // accelerometer_data_t accel_data = {.ax = accel.x / 100.0f, .ay = accel.y / 100.0f, .az = accel.z / 100.0f}; // 举例的加速度计读数

//         // e_angles = mpu_get_eulerian_angles(gyro_data[0], gyro_data[1], gyro_data[2], acc_data[0], acc_data[1], acc_data[2]);

//         // printf("ax = %f, ay = %f, az = %f\n", accel_data.ax, accel_data.ay, accel_data.az);
//         // calculateInstantaneousAcceleration(e_angles, accel_data, &instantaneous_acceleration);

//         // 输出瞬时加速度
//         // printf("Instantaneous Acceleration: Ax=%f, Ay=%f, Az=%f\n", instantaneous_acceleration.ax, instantaneous_acceleration.ay, instantaneous_acceleration.az);

//         rt_thread_mdelay(5);
//     }

//     return RT_EOK;
// }
